package edu.wpi.first.wpilibj.op;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.frc.Robot;
import edu.wpi.first.wpilibj.mechanism.PitchingMechanism;
import edu.wpi.first.wpilibj.positioning.Coordinates;
import edu.wpi.first.wpilibj.positioning.ManualPathfinder;
import edu.wpi.first.wpilibj.positioning.Path;
import edu.wpi.first.wpilibj.positioning.Pathfinder12;

/**
 * The 2012 implementation of Teleop, this class is a SINGLETON, and contains
 * methods that will control the robot's actions.
 * @author Dylan Frese
 */
public final class Teleop12 implements Op {

    /**
     * Our sticks of joy.
     */
    private Joystick joy1, joy2;
    /**
     * This is the ONLY instance of this class. It can not be changed, nor will
     * a new one be created.
     */
    private static final Teleop12 SINGLETON = new Teleop12();

    /**
     * Gets the only instance of this class (a SINGLETON).
     * @return 
     */
    public static Teleop12 getInstance() {
        return SINGLETON;
    }

    /**
     * Construct the Teleop12 Class. This is only called once, and is private.
     */
    private Teleop12() {
        joy1 = new Joystick(5);
        
    }
    /**
     * The {@link ManualPathfinder} the class uses to navigate.
     */
    private ManualPathfinder pathfinder12 = new Pathfinder12();

    /**
     * Gets the current {@link ManualPathfinder} the Teleop12 class is using to navigate.
     * @return 
     */
    public ManualPathfinder getCurrentPathfinder() {
        return pathfinder12;
    }

    /**
     * {@inheritDoc}
     */
    @Override
    public void tick() {
        //Get the current robot's path, passing in the two joysticks as input
        Path p = getCurrentPathfinder().getPath(Robot.robot.getWhere(), System.currentTimeMillis(), joy1, joy2);
        //While there is more of the path,
        while (p.hasNext()) {
            //Get the next coordinates...
            Coordinates c = p.next();
            //And go to them.
            Robot.robot.gotoCoords(c);
        }
        //Declared here to save characters.
        final PitchingMechanism pitchMech = Robot.robot.pitchingMachine;
        //If the button wasn't pressed before, and is now,
        if (!buttonOnePressed && joy2.getRawButton(1)) {
            //The button has been pressed.
            buttonOnePressed = true;
            //Toggle the running-state of the Pitching Machine
            if (pitchMech.isRunning()) {
                pitchMech.stop();
            } else {
                pitchMech.start();
            }
        } else {
            //If it's not pressed, than set the variable storing that state to 'not pressed'.
            buttonOnePressed = false;
        }
    }
    
    private boolean buttonOnePressed;
    
}
